import numpy as np


EPS = 1e-4

def relative_coord(x: np.ndarray,xref: np.ndarray):
    # 计算相对坐标
    return np.float64([x[0]-xref[0],x[1]-xref[1]])

def chans_method(anchor1: np.ndarray,
          anchor2: np.ndarray,
          anchor3: np.ndarray,
          r_21: float,
          r_31: float
          ):
    # Chans法TDOA计算坐标
    # 需要三个采样点坐标和两个距离差
    # 提取坐标
    x1,y1 = anchor1[0],anchor1[1]
    x2,y2 = anchor2[0]+EPS,anchor2[1]
    x3,y3 = anchor3[0],anchor3[1]+EPS
    # 计算Chans法中所需的几个矩阵
    # 这里使用的是伪逆矩阵（有可能行列式值为0）
    p1 = - np.linalg.inv(np.array(
    [[x2-x1,y2-y1],
     [x3-x1,y3-y1]]
     ,dtype=np.float64))
    p2 = np.array(
    [[r_21],
     [r_31]]
    ,dtype=np.float64)
    k1 = x1**2 + y1**2
    k2 = x2**2 + y2**2
    k3 = x3**2 + y3**2
    p3 = np.array(
    [[k1+r_21**2-k2],
     [k1+r_31**2-k3]]
    ,dtype=np.float64) / 2.0
    a1 = np.array(
    [[x1],
     [y1]]
     ,dtype=np.float64)
    # 与r1相关的二元一次方程组的系数
    a = (p1.dot(p2).T.dot(p1.dot(p2))-1)[0][0]
    b = (p1.dot(p2).T.dot(p1.dot(p3)-a1) + (p1.dot(p3)-a1).T.dot(p1).dot(p2)
        )[0][0]
    c = ((p1.dot(p3)-a1).T.dot(p1.dot(p3)-a1))[0][0]
    # 求解二元一次方程组（只需要一个解，存疑）
    r1 = (-b-(b**2-4*a*c)**0.5)/2.0/ a
    real_coord = p1.dot(p2)*r1 + p1.dot(p3)
    # 返回估测距离，在坐标系中的坐标
    return r1,real_coord.T[0]
    
if __name__ == '__main__':
    print(chans_method(
            np.float64([0,0]),
            np.float64([-0.3,0]),
            np.float64([0.3,0]),
            -0.121,
            0.192
        ))